//#include <omp.h>
#include "liblpa.h"
#include <stdint.h>
//#include <smmintrin.h>
// Is it valid outside GCC?
#include <x86intrin.h> // Activate SSE4 intrinsics

#define OMP_PARA_INTERNAL _Pragma("omp parallel for schedule (static)")
//#define OMP_PARA_INTERNAL ;

#define lpaCoreI(func, left, right, size) {                                  \
    unsigned int i;                                                          \
    __m128i m128_leftr, m128_rightr, m128_result;                            \
    OMP_PARA_INTERNAL                                                        \
    for (i=0; i<size; ++i) {                                                 \
        m128_leftr = _mm_loadu_si128(left + i * sizeof(__m128i));            \
        m128_rightr = _mm_loadu_si128(right + i * sizeof(__m128i));          \
        m128_result = func(m128_leftr, m128_rightr);                         \
        _mm_storeu_si128((void *)(left + i * sizeof(__m128i)), m128_result); \
    }                                                                        \
}

#define lpaCoreF(func, left, right, size) {                                  \
    unsigned int i;                                                          \
    __m128 m128_leftr, m128_rightr, m128_result;                             \
    OMP_PARA_INTERNAL                                                        \
    for (i=0; i<size; ++i) {                                                 \
        m128_leftr = _mm_loadu_ps((float*)(left + i * sizeof(__m128)));      \
        m128_rightr = _mm_loadu_ps((float*)(right + i * sizeof(__m128)));    \
        m128_result = (*func)(m128_leftr, m128_rightr);                      \
        _mm_storeu_ps((void *)(left + i * sizeof(__m128)), m128_result);     \
    }                                                                        \
}

#define lpaCoreD(func, left, right, size) {                                  \
    unsigned int i;                                                          \
    __m128d m128_leftr, m128_rightr, m128_result;                            \
    OMP_PARA_INTERNAL                                                        \
    for (i=0; i<size; ++i) {                                                 \
        m128_leftr = _mm_loadu_pd((double*)(left + i * sizeof(__m128d)));    \
        m128_rightr = _mm_loadu_pd((double*)(right + i * sizeof(__m128d)));  \
        m128_result = (*func)(m128_leftr, m128_rightr);                      \
        _mm_storeu_pd((void *)(left + i * sizeof(__m128d)), m128_result);    \
    }                                                                        \
}

#define lpaCoreI1(func, left, size) {                                        \
    unsigned int i;                                                          \
    __m128i m128_leftr, m128_result;                                         \
    OMP_PARA_INTERNAL                                                        \
    for (i=0; i<size; ++i) {                                                 \
        m128_leftr = _mm_loadu_si128(left + i * sizeof(__m128i));            \
        m128_result = func(m128_leftr);                                      \
        _mm_storeu_si128((void *)(left + i * sizeof(__m128i)), m128_result); \
    }                                                                        \
}

#define lpaCoreF1(func, left, size) {                                        \
    unsigned int i;                                                          \
    __m128 m128_leftr, m128_result;                                          \
    OMP_PARA_INTERNAL                                                        \
    for (i=0; i<size; ++i) {                                                 \
        m128_leftr = _mm_loadu_ps((float*)(left + i * sizeof(__m128)));      \
        m128_result = (*func)(m128_leftr);                                   \
        _mm_storeu_ps((void *)(left + i * sizeof(__m128)), m128_result);     \
    }                                                                        \
}

#define lpaCoreD1(func, left, size) {                                        \
    unsigned int i;                                                          \
    __m128d m128_leftr, m128_result;                                         \
    OMP_PARA_INTERNAL                                                        \
    for (i=0; i<size; ++i) {                                                 \
        m128_leftr = _mm_loadu_pd((double*)(left + i * sizeof(__m128d)));    \
        m128_result = (*func)(m128_leftr);                                   \
        _mm_storeu_pd((void *)(left + i * sizeof(__m128d)), m128_result);    \
    }                                                                        \
}

#define FloatSwitch(func)                                                    \
        case 8 | LPA_FLOAT:                                                  \
            lpaCoreD(func##_pd, left, right, size/2);                        \
            return;                                                          \
        case 4 | LPA_FLOAT:                                                  \
            lpaCoreF(func##_ps, left, right, size/4);                        \
            return;

#define IntegerSwitch(func)                                                  \
        case 8:                                                              \
            lpaCoreI(func##64, left, right, size/2);                         \
            return;                                                          \
        case 4:                                                              \
            lpaCoreI(func##32, left, right, size/4);                         \
            return;                                                          \
        case 2:                                                              \
            lpaCoreI(func##16, left, right, size/8);                         \
            return;                                                          \
        case 1:                                                              \
        default:                                                             \
            lpaCoreI(func##8, left, right, size/16);                         \

#define VarTypeSwitch(intfunc, floatfunc) {                                  \
    switch(vartype) {                                                        \
        FloatSwitch(floatfunc)                                               \
        IntegerSwitch(intfunc)                                               \
    }                                                                        \
}

#define FloatSwitch1(func)                                                   \
        case 8 | LPA_FLOAT:                                                  \
            lpaCoreD1(func##_pd, left, size/2);                              \
            return;                                                          \
        case 4 | LPA_FLOAT:                                                  \
            lpaCoreF1(func##_ps, left, size/4);                              \
            return;                                                          \

#define IntegerSwitch1(func)                                                 \
        case 8:                                                              \
            lpaCoreI1(func##64, left, size/2);                               \
            return;                                                          \
        case 4:                                                              \
            lpaCoreI1(func##32, left, size/4);                               \
            return;                                                          \
        case 2:                                                              \
            lpaCoreI1(func##16, left, size/8);                               \
            return;                                                          \
        case 1:                                                              \
        default:                                                             \
            lpaCoreI1(func##8, left, size/16);                               \

#define VarTypeSwitch1(intfunc, floatfunc) {                                 \
    switch(vartype) {                                                        \
        FloatSwitch1(floatfunc)                                              \
        IntegerSwitch1(intfunc)                                              \
    }                                                                        \
}

#define ApiFunc(funcname, integer_intrin, float_intrin)                             \
void funcname(void *left, void *right, unsigned int size, unsigned short vartype) { \
    VarTypeSwitch(integer_intrin, float_intrin);                                    \
}

#define ApiFunc1(funcname, integer_intrin, float_intrin)                            \
void funcname(void *left, unsigned int size, unsigned short vartype) {              \
    VarTypeSwitch1(integer_intrin, float_intrin);                                   \
}

/*
 * Arithmetic section
 */
ApiFunc(lpaAdd, _mm_add_epi, _mm_add)

// EXTREME TODO: fix 64bit!
#define _mm_abs_epi64 _mm_abs_epi32
void lpaAbs(void *left, unsigned int size, unsigned short vartype) {
    unsigned int i;
    switch(vartype) {
        case 8 | LPA_FLOAT:
        case 4 | LPA_FLOAT:
            // TODO: Use lpaAndNot
            OMP_PARA_INTERNAL
            for (i=0; i<size*vartype; i += (vartype & 0xFF)) {
                // Compare to _mm_andnot_ps
                ((unsigned char*)left)[i] &= 0x7f; // Nulls the sign bit (see IEEE 754-2008 )
            }
            return;
        IntegerSwitch1(_mm_abs_epi)
    }
}

//void lpaAbs(void *left, unsigned int size, unsigned short vartype) {
//    unsigned int i;
//    switch(vartype) {
//        case 8:
//            #pragma omp parallel for schedule (static) if (size > 100)
//            for (i=0; i<size; i+=1) {
//                ((int64_t*)left)[i] = -((int64_t*)left)[i];
//            }
//            return;
//        case 4:
//            #pragma omp parallel for schedule (static) if (size > 100)
//            for (i=0; i<size; i+=1) {
//                ((int32_t*)left)[i] = -((int32_t*)left)[i];
//            }
//            return;
//        case 2:
//            #pragma omp parallel for schedule (static) if (size > 100)
//            for (i=0; i<size; i+=1) {
//                ((int16_t*)left)[i] = -((int16_t*)left)[i];
//            }
//            return;
//        case 1:
//        default:
//            #pragma omp parallel for schedule (static) if (size > 100)
//            for (i=0; i<size; i+=1) {
//                ((int8_t*)left)[i] = -((int8_t*)left)[i];
//            }
//    }
//}

ApiFunc(lpaSub, _mm_sub_epi, _mm_sub)

// EXTREME TODO: fix this!
#define _mm_mullo_epi64 _mm_mullo_epi32
#define _mm_mullo_epi8 _mm_mullo_epi16
ApiFunc(lpaMul, _mm_mullo_epi, _mm_mul)

void lpaDiv(void *left, void *right, unsigned int size, unsigned short vartype) {
    // TODO: use libdivide
    unsigned int i;
    switch(vartype) {
        FloatSwitch(_mm_div)
        case 8:
            OMP_PARA_INTERNAL
            for (i=0; i<size; ++i) {
                ((uint64_t*)left)[i] /= ((uint64_t*)right)[i];
            }
            return;
        case 4:
            OMP_PARA_INTERNAL
            for (i=0; i<size; ++i) {
                ((uint32_t*)left)[i] /= ((uint32_t*)right)[i];
            }
            return;
        case 2:
            OMP_PARA_INTERNAL
            for (i=0; i<size; ++i) {
                ((uint16_t*)left)[i] /= ((uint16_t*)right)[i];
            }
            return;
        case 1:
        default:
            OMP_PARA_INTERNAL
            for (i=0; i<size; ++i) {
                ((uint8_t*)left)[i] /= ((uint8_t*)right)[i];
            }
    }
}

// rsqrt isn't a good enough approximation for doubles, reverting to sqrt
#define _mm_rsqrt_pd _mm_sqrt_pd
// EXTREME TODO: fix integer!
#define _mm_abs_epi64 _mm_abs_epi32
ApiFunc1(lpaSqrt, _mm_abs_epi, _mm_rsqrt)

/*
 * Logical section
 */

// TODO: Redo that, it's terrible
#define _mm_and_si64 _mm_and_si128
#define _mm_and_si32 _mm_and_si128
#define _mm_and_si16 _mm_and_si128
#define _mm_and_si8 _mm_and_si128
#define _mm_andnot_si64 _mm_andnot_si128
#define _mm_andnot_si32 _mm_andnot_si128
#define _mm_andnot_si16 _mm_andnot_si128
#define _mm_andnot_si8 _mm_andnot_si128
#define _mm_or_si64 _mm_or_si128
#define _mm_or_si32 _mm_or_si128
#define _mm_or_si16 _mm_or_si128
#define _mm_or_si8 _mm_or_si128
#define _mm_xor_si64 _mm_xor_si128
#define _mm_xor_si32 _mm_xor_si128
#define _mm_xor_si16 _mm_xor_si128
#define _mm_xor_si8 _mm_xor_si128

//ApiFunc1(lpaNot, _mm_not_si, _mm_not)
ApiFunc(lpaAnd, _mm_and_si, _mm_and)
ApiFunc(lpaAndNot, _mm_andnot_si, _mm_andnot)
ApiFunc(lpaOr, _mm_or_si, _mm_or)
ApiFunc(lpaXOr, _mm_xor_si, _mm_xor)

char lpaCmp(void *restrict left, void *restrict right, unsigned int size, unsigned short vartype) {
    unsigned int i;
    char abort = 0;
    __m128i m128_leftr, m128_rightr, m128_result;

    size = size / (16 / vartype);

//    OMP_PARA_INTERNAL
    for (i=0; i<size; ++i) {
//        #pragma omp flush (abort)
        if (!abort) {
            m128_leftr = _mm_loadu_si128(left + i * sizeof(__m128i));
            m128_rightr = _mm_loadu_si128(right + i * sizeof(__m128i));
//            m128_result = _mm_cmpeq_ps(m128_leftr, m128_rightr);
//            if (_mm_movemask_epi8(_mm_cmpeq_epi8(xmm, _mm_setzero_si128())) == 0xFFFF) {
//                return 1;
//            }
            // Optimize this and compare it to cmpeqps
            m128_result = _mm_cmpeq_epi64(m128_leftr, m128_rightr);
            if (((int64_t*)&m128_result)[0] == 0 || ((int64_t*)&m128_result)[1] == 0) {
//              if (_mm_cmpistri(m128_leftr, m128_rightr, 16)) {
                abort = 1;
                return 1;
//                #pragma omp flush (abort)
            }
        }
    }
    return 0;
}

/*
 * Reduce section
 */
#ifdef __XOP__
// EXTREME TODO: Fix this!
#define _mm_hadd_epi64 _mm_haddq_epi32
#define _mm_hadd_epi32 _mm_haddq_epi32
#define _mm_hadd_epi16 _mm_haddd_epi16
#define _mm_hadd_epi8 _mm_haddw_epi8
ApiFuncI1(lpaHadd, _mm_hadd_epi)

void lpaSum(void *left, unsigned int size, unsigned short vartype) {
    while(1) {
        lpaHadd(left, size, vartype);
        // Gotta play with shuffles...
        size /= (16 / vartype);
        if (size <= 1)
            break;
    }
}
#endif

void lpaDotProduct(void *restrict left, void *restrict right, unsigned int size, unsigned short vartype);

/*
 * High-level section
 */

void lpaConvolution(void *restrict left, void *restrict right, unsigned int sizeleft, unsigned int sizeright, unsigned short vartype) {
    unsigned long i;
    #pragma omp parallel for schedule (static) if (sizeleft > 100)
    for (i=0; i<sizeleft; i+=1) {
        lpaMul(left + i, right, sizeright, vartype);
    }
}
